import lejos.nxt.UltrasonicSensor;

public class ObstacleDetector extends Thread {
	private UltrasonicSensor sensor = new UltrasonicSensor(
			Configuration.ULTRASONICSENSOR_PORT);
	private WaitController waitController;

	public ObstacleDetector(WaitController waitController) {
		this.waitController = waitController;
		setName("UltraSonicController");
		setDaemon(true);
		setPriority(NORM_PRIORITY);

		// Force sensor in ping mode.
		sensor.continuous();
		sensor.ping();

		start();
	}

	public void run() {
		boolean foundObject = false;
		int[] dists = new int[8];
		while (true) {
			try {
				Thread.sleep(100);
				sensor.ping();
				sensor.getDistances(dists);
				
				int sum = 0, count = 0;
				for (int i=0 ; i < 8 ; i++) {
					if (dists[i] != 255) {
						sum += dists[i];
						count++;
					}
				}
				
				int avg;
				if (count > 0) {
					avg = sum / count;
				} else {
					avg = 255;
				}
				
				if (!foundObject && avg < Configuration.MINIMUM_SAFE_DISTANCE) {
					waitController.obstacleFound();
					foundObject = true;
					Thread.sleep(2000);
				} else if (foundObject && avg > Configuration.MINIMUM_SAFE_DISTANCE) {
					waitController.obstacleCleared();
					foundObject = false;
				}

			} catch (InterruptedException e) {
				System.out.println("<UltraSonic Interrupted>");
			}
		}
	}
}